/**************************************************************************************************
* Title: SumVector2_16f.cpp
* Author: Gael Huber
* Description: Data structure containing 16-byte aligned 2-dimensional vector and its various 
*	operations.
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Hermite interpolation between position V1, tangent T1 (when s == 0) and position V2, tangent T2 
* (when s == 1).
**************************************************************************************************/
Vector2_16f* Vec2Hermite_16f(Vector2_16f* pOut, const Vector2_16f* pV1, const Vector2_16f* pT1, 
	const Vector2_16f* pV2, const Vector2_16f* pT2, float s) {
	// Calculate a, b, c, d values (these are more efficiently calculated without using SIMD intrinsics
	float a(2.0f*s*s*s - 3.0f*s*s + 1.0f);
	float b(-2.0f*s*s*s + 3.0f*s*s);
	float c(s*s*s - 2.0f*s*s + s);
	float d(s*s*s - s*s);

	// Solve for a*v1 + b*v2 + c*t1 + d*t2
	Vector2_16f v1, v2;
	
	Vec2Scale_16f(&v1, pV1, a);
	Vec2Scale_16f(&v2, pV2, b);
	Vec2Add_16f(pOut, &v1, &v2);

	Vec2Scale_16f(&v1, pT1, c);
	Vec2Add_16f(pOut, pOut, &v1);

	Vec2Scale_16f(&v1, pT2, d);
	Vec2Add_16f(pOut, pOut, &v1);

	return pOut;
}

/**************************************************************************************************
* CatmullRom interpolation between V1 (when s == 0) and V2 (when s == 1)
**************************************************************************************************/
Vector2_16f* Vec2CatmullRom_16f(Vector2_16f* pOut, const Vector2_16f* pV0, const Vector2_16f* pV1,
	const Vector2_16f* pV2, const Vector2_16f* pV3, float s) {
	// Calculate a, b, c, d
	float a(-s*s*s + 2.0f*s*s - s);
	float b(3.0f*s*s*s - 5.0f*s*s + 2.0f);
	float c(-3.0f*s*s*s + 4.0f*s*s + s);
	float d(s*s*s - s*s);

	// Solve for (a*p1 + b*p2 + c*p3 + d*p4)/2
	Vector2_16f v1, v2;

	Vec2Scale_16f(&v1, pV0, a);
	Vec2Scale_16f(&v2, pV1, b);
	Vec2Add_16f(pOut, &v1, &v2);

	Vec2Scale_16f(&v1, pV2, c);
	Vec2Add_16f(pOut, pOut, &v1);

	Vec2Scale_16f(&v1, pV3, d);
	Vec2Add_16f(pOut, pOut, &v1);

	Vec2Scale_16f(pOut, pOut, 0.5f);

	return pOut;
}

/**************************************************************************************************
* Barycentric coordinates V1 + f(V2 - V1) + g(V3 - V1)
**************************************************************************************************/
Vector2_16f* Vec2Barycentric(Vector2_16f* pOut, const Vector2_16f* pV1, const Vector2_16f* pV2,
	const Vector2_16f* pV3, float f, float g) {
	pOut->v = _mm_add_ps(pV1->v, 
		_mm_add_ps(
			_mm_mul_ps(_mm_set1_ps(f), _mm_sub_ps(pV2->v, pV1->v)), 
			_mm_mul_ps(_mm_set1_ps(g), _mm_sub_ps(pV3->v, pV1->v))));
	return pOut;
}

/**************************************************************************************************
* Transform by matrix (x, y, 0, 1)
**************************************************************************************************/
Vector4_16f* Vec2Transform_16f(Vector4_16f* pOut, const Vector2_16f* pV, const Matrix_16f* pM) {
	pOut->v = _mm_add_ps(_mm_add_ps(
		_mm_mul_ps(_mm_set_ps(pM->_11, pM->_21, pM->_31, pM->_41), _mm_set1_ps(pV->x)),
		_mm_mul_ps(_mm_set_ps(pM->_12, pM->_22, pM->_32, pM->_42), _mm_set1_ps(pV->y))),
		_mm_set_ps(pM->_14, pM->_24, pM->_34, pM->_44));
	return pOut;
}

/**************************************************************************************************
* Transform coords by matrix (x, y, 0, 1)
**************************************************************************************************/
Vector2_16f* Vec2TransformCoord_16f(Vector2_16f* pOut, const Vector2_16f* pV, const Matrix_16f* pM) {
	pOut->v = _mm_add_ps(_mm_add_ps(
		_mm_mul_ps(_mm_set_ps(pM->_11, pM->_21, pM->_31, pM->_41), _mm_set1_ps(pV->x)),
		_mm_mul_ps(_mm_set_ps(pM->_12, pM->_22, pM->_32, pM->_42), _mm_set1_ps(pV->y))),
		_mm_set_ps(pM->_14, pM->_24, pM->_34, pM->_44));
	return pOut;
}

/**************************************************************************************************
* Transform normal by matrix (x, y, 0, 0)
**************************************************************************************************/
Vector2_16f* Vec2TransformNormal_16f(Vector2_16f* pOut, const Vector2_16f* pV, const Matrix_16f* pM) {
	pOut->v = _mm_add_ps(
		_mm_mul_ps(_mm_set_ps(pM->_11, pM->_21, pM->_31, pM->_41), _mm_set1_ps(pV->x)),
		_mm_mul_ps(_mm_set_ps(pM->_12, pM->_22, pM->_32, pM->_42), _mm_set1_ps(pV->y)));
	return pOut;
}